%% 二维CFAR检测STFT图像
% 检查是否外部运行
if exist('outsideRun','var')
    if islogical(outsideRun)
        if outsideRun
            disp('外部运行，不加载变量');
        else
            disp('外部运行，加载变量');
        end
    else
        outsideRun = false;
        disp('主程序运行');
    end
else
    outsideRun = false;
    disp('主程序运行');
end
% 清理变量
if not(outsideRun)
    clc;
    close('all','force');
    clearvars;
    outsideRun = false;
end
% 是否绘图
drawFlag = true;
if outsideRun
    drawFlag = false;
end
%% 图片保存位置
ifSave = false;
savePath = 'D:\MDCurveExtractFigs';
if and(ifSave,not(isfolder(savePath)))
    disp(strcat('若要保存，在运行前请确保保存路径',savePath,'存在！'));
end
%% 雷达模型参数
lightSpeed  = 3e8;    % 光速
timeLen = 0.3;    % 仿真持续时间
%% 导入参考回波数据
sampleFreq = 1e6;    % 信号采样率
carrFreq = 5e9;        % 载波频率，ku波段
frameFreq = 20e3;    % 脉冲重复频率
framePeriod = 1/frameFreq;   % 相干处理时间：4ms，即测量间隔
frameNums = round(timeLen * frameFreq);  % 总采样点数
%% 测量模型参数
bladeNums   = 4;
bladeLen  = 5;
bladeRateHz = 5.5;
bladeRate = deg2rad(bladeRateHz*360);  % rps -> rad/sec  叶片旋转角速度
dopplerFreq = 0;  % 正常多普勒的频率，Hz
dopplerAmp = 1;     % 正常多普勒的幅度
%% variable
dopplerPhase = 0;    % % 正常多普勒的初始相位

echo = zeros([1,frameNums]);
doppler = zeros(bladeNums + 1,frameNums);
bladePhase = 0;
%% generate measurements
% 状态维持（七维）：bladeNums、bladeLen、bladePhase、bladeRate、dopplerPhase、dopplerFreq、dopplerAmp
for frameIdx = 1:frameNums
    echo(1,frameIdx) = mdMeasEcho_new(lightSpeed, carrFreq,...
        bladeNums, bladeLen, bladePhase,...
        dopplerPhase, dopplerAmp);   % 仿真信号回波
    doppler(2:end,frameIdx) = md_new(lightSpeed, carrFreq,...
        bladeNums, bladeLen, bladePhase) * bladeRateHz + dopplerFreq;  % 计算微多普勒频移
    doppler(1,frameIdx) = dopplerFreq;   % 主体回波频移
    
    bladePhase = bladePhase + bladeRate * framePeriod;
    dopplerPhase = dopplerPhase + dopplerFreq * framePeriod;
end
%% stft
% echo_noise = echo;
SNRdb = 40;  % 信噪比，单位db
if outsideRun   % 外部运行调用SNR变量
    SNRdb = SNR;  % 信噪比，单位db
end
echo_noise = awgn(echo,SNRdb,'measured');   % 必须添加噪声，否则cfar结果不准确
[timeFreqImg,timeFreq_labelF,timeFreq_labelT] = pspectrum(echo_noise, frameFreq, 'spectrogram');
% pspectrum的内部算法无法推出，这里假设timeFreq_labelT表示该帧的中间时刻，
% 为保证帧数量为整数，从输入信号的前面开始留空，
% 留到总采样数可被帧内fft点数整除为止，
% 也就是说，帧内fft点数为总采样点数减去最后一个timeFreq_labelT的值乘以采样率的两倍，
% 这里试出来为128点，帧与帧之间有重叠，保证总帧数为128的基础上进行间隔计算。
%% 参数
% ----cfar检测
NumTrainingCells = 42;   % cfar训练单元格，应为偶数42
NumGuardCells = 38;   % cfar保护单元格38
cfarPfa = 1e-3;    % cfar的虚警率1e-3

% ----dbscan对索引聚类
dbscanEpsilon = 17;  % 邻居距离17
dbscanMinpts = 8;    % 最小点数8

% ----根据统计曲线数量结果将各条曲线中不合理的关键点去除
frontCompFrames = 10;  % 对每一关键帧的聚类结果，与其前frontCompFrames帧比较，若其满足小于最大值，大于最小值，则认为有效
freqBias = 0.01;  % 最大最小值的偏差比例，即若该关键帧曲线索引小于前几关键帧曲线频率最大索引+bias并小于最小索引-bias，则有效

% ----提取曲线
freqSearchBias = 0.03;   % 以该比例乘以总频率单元格数在上一帧单元格两边进行频率搜索

% ----找交叉点
freqCross = 0.04;   % 以该比例乘以总频率单元格数作为判定交叉点的距离

% ----合并交叉点
mergeFreq = 0.05;   % 需要合并交叉点的平均频率单元格间距的比例
mergeTime = 3;   % 需要合并交叉点的帧数单元格间距，这里必须大于等于1，否则两个交叉点连着会出bug
mergePts = 2;   % 一个交叉点至少有mergePts个涉及点才算为交叉点

% ----计算各交叉点前后配对曲线索引
diffMeanBins = 5;    % 计算左右侧斜率使用单元格（帧）数量
diffFreqBias = 0.001;    % 该值为频率向上比例值，交叉点左右斜率偏差小于等于该值对应单元格数量都认为斜率一致
%% 变量
% TFILineNum = 0;    % 视频图中有几个散射点形成的轨迹

TFI_cfar = zeros(size(timeFreqImg));    % cfar标签，置零为
TFI_dbscan = -1 * ones(size(timeFreqImg));   % 聚类标签，全置-1表示没有类
dbscanGroupsBuffer = zeros([1,size(timeFreqImg,2)]);% 类数缓冲器，缓冲各帧有几类
frameCut = zeros([1,size(timeFreqImg,2)]);   % 以正确类数帧为中心对帧的切分
    % 设frameCutIdx = [3,5]，则表示将帧切分为frameCut = [1,1,1,2,2]
%% cfar检测
% NumTrainingCells = 42;   % cfar训练单元格，应为偶数
% NumGuardCells = 38;   % cfar保护单元格
% cfarPfa = 1e-3;    % cfar的虚警率

% ------定义matlabcfar检测器
cfar = phased.CFARDetector('NumTrainingCells',NumTrainingCells,'NumGuardCells',NumGuardCells);
cfar.ThresholdFactor = 'Auto';
cfar.ProbabilityFalseAlarm = cfarPfa;
cfar.ThresholdOutputPort = true;

cfarCutIdx = (NumTrainingCells+NumGuardCells+1):(size(timeFreqImg,1)-NumTrainingCells-NumGuardCells);
    % cfar需要将两边切掉一部分，这是检测部分的数值索引
    % 这里可以改进，对于超出1/2采样频率的微多普勒频移，其表现为频谱的混叠，
    % 在二维图片上的表现即为曲线超出图片的上（下）沿，
    % 并从图片的下（上）沿在消失时刻的下一时刻重新出现并继续衍生，
    % 这里直接将两边的频率裁剪去掉的方法是无法提取到这样混叠的曲线的，
    % 需要对二维图做循环补偿，然后cfar方法才可以使用。

% 对时频图各帧进行cfar检测
for timeIdx = 1:size(timeFreqImg,2)
    freqLine = abs(timeFreqImg(:,timeIdx));  % cfar需要abs
    
    [cfarDet,cfarTh] = cfar(freqLine,cfarCutIdx);
    TFI_cfar(cfarCutIdx,timeIdx) = cfarDet;
end
%% dbscan对索引聚类
% dbscanEpsilon = 17;  % 邻居距离
% dbscanMinpts = 8;    % 最小点数

for timeIdx = 1:size(timeFreqImg,2)
    cfarDet = TFI_cfar(:,timeIdx);
    cfarDetIdx = find(cfarDet);   % 给出检测的数值索引
    
    
    if (~isempty(cfarDetIdx))   % 为空报错，确保非空
        dbscanIdx = dbscan(cfarDetIdx,dbscanEpsilon,dbscanMinpts);   % 这里的参数需要自适应计算，后期加入
        TFI_dbscan(cfarDetIdx,timeIdx) = dbscanIdx;
        dbscanIdxTarget = (dbscanIdx >= 0);  % dbscanIdx中大于0的索引表示目标
        if (any(dbscanIdxTarget))
            dbscanGroupsBuffer(1,timeIdx) = max(dbscanIdx(dbscanIdxTarget));
        end
    end
    
end

%% 统计
[histNums,histEdges] = histcounts(dbscanGroupsBuffer,1:max(dbscanGroupsBuffer));
    % 对聚类数做统计，计算最多出现次数的类数，将该数作为图中有几类曲线的证据
[histMaxValue,histMaxIdx] = max(histNums);
TFILineNum = histMaxIdx;

% 展示结果
% disp(strcat('统计直方图边界=',mat2str(histEdges)));
% disp(strcat('统计直方图计数=',mat2str(histNums)));
% disp(strcat('统计直方图计数最大=',mat2str(histMaxValue)));
% disp(strcat('统计直方图计数最大对应边界=',mat2str(histEdges(1,histMaxIdx))));
disp(strcat('曲线条数=',num2str(TFILineNum)));

%% 提取关键帧
correctFrames_origin = find(dbscanGroupsBuffer == TFILineNum);
%% 根据统计曲线数量结果将各条曲线中不合理的关键点去除
% frontCompFrames = 10;  % 对每一关键帧的聚类结果，与其前frontCompFrames帧比较，若其满足小于最大值，大于最小值，则认为有效
freqBiasBins = floor(freqBias * size(timeFreqImg,1));   % 最大最小值的偏差，即若该关键帧曲线索引小于前几关键帧曲线频率最大索引+bias并小于最小索引-bias，则有效

correctFrames_new = zeros(size(correctFrames_origin));   % 先暂时复制一遍
correctFrames_new_conut = 0;   % 总共修正后的正确帧数量

for frameCutNum = 1:length(correctFrames_origin)
    if frameCutNum <= frontCompFrames
        correctFrames_new_conut = correctFrames_new_conut + 1;
        correctFrames_new(1,correctFrames_new_conut) = correctFrames_origin(1,frameCutNum);
        continue;
    end
    
    compStartIdx = frameCutNum - 10;
    compEndIdx = frameCutNum - 1;
    if compStartIdx < 1
        compStartIdx = 1;
    end
    if compEndIdx < 1
        compEndIdx = 1;
    end
    
    compValid = true;   % 比较合格缓冲器
    
    compCorrectFrames = correctFrames_origin(1,compStartIdx:compEndIdx);  % 需要对比的前几帧
    compTFI = TFI_dbscan(:,compCorrectFrames);   % 待对比的前几帧聚类结果
    % 分曲线对比
    for TFILineSubIdx = 1:TFILineNum
        compFreqBins = find(sum((compTFI == TFILineSubIdx),2));  % 前几关键帧该条曲线所有单元格
        compFreqMin = min(compFreqBins);  % 最小频率单元格索引
        compFreqMax = max(compFreqBins);  % 最大频率单元格索引
        
        TFI_frameIdxMat = (TFI_dbscan(:,correctFrames_origin(1,frameCutNum)) == TFILineSubIdx);
            % 为取出该关键帧所对应第几条曲线的所有聚类点，提取最大值用
        
        [TFI_frameMax,TFI_frameIdx] = max(...
            timeFreqImg(TFI_frameIdxMat,correctFrames_origin(1,frameCutNum))...
            );   % 该关键帧中第几条曲线的局部最大值索引
        
        TFI_frameIdx = TFI_frameIdx + find(TFI_frameIdxMat,1,'first') - 1;  % 修正局部最大值索引到全局
        
        if ((TFI_frameIdx <= (compFreqMax + freqBiasBins))...
                && (TFI_frameIdx >= (compFreqMin - freqBiasBins)))
            % 该关键点合格
            compValid = compValid && true;
        else
            compValid = compValid && false;
            break;
        end
    end
    
    if compValid
        correctFrames_new_conut = correctFrames_new_conut + 1;
        correctFrames_new(1,correctFrames_new_conut) = correctFrames_origin(1,frameCutNum);
    end
end

correctFrames = correctFrames_new(1,1:correctFrames_new_conut);
    % 将修正关键帧中无用的裁减掉，作为新的关键帧
%% 分段
if length(correctFrames) >= 2
    frameCutTmp = (correctFrames(2:end) + correctFrames(1:(end-1))) / 2;
    frameCutIdx = ceil(frameCutTmp) - 1;
else
    frameCutIdx = correctFrames;
end
% frameCutIdx为切分关键帧，例如为[3,5]，则表示将帧切分为[1,1,1,2,2]

frameCutIdxCount = 1;
for timeIdx = 1:size(timeFreqImg,2)
    frameCut(1,timeIdx) = frameCutIdxCount;
    
    if (frameCutIdxCount <= length(frameCutIdx))...
            && (timeIdx >= frameCutIdx(frameCutIdxCount))
        % 需要确保frameCutIdxCount不超过关键帧的数量，因为关键帧可能不到
        % 所有帧的末尾，这样末尾就直接全部切为最后一个关键帧之内
        frameCutIdxCount = frameCutIdxCount + 1;  % 超过切分关键帧数字，count加一
    end
end
%% 提取曲线
% freqSearchBias = 0.03;   % 以该比例乘以总频率单元格数在上一帧单元格两边进行频率搜索
freqSearchBiasBins = floor(freqSearchBias * size(timeFreqImg,1));
    % 上述频率搜索单元格数

% frameCut为切分后以关键帧为中心的分段
% frameCutIdx为切分帧
% correctFrames为关键帧索引
TFILineIdx = int32(zeros([TFILineNum,size(timeFreqImg,2)]));
    % 提取结果，每行表示一条曲线，列数为帧数，数值表示时频图中行索引即频率索引
frameCutIdxFront = [0,frameCutIdx] + 1;   % 切分段起始索引
frameCutLen = diff([0,frameCutIdx,size(timeFreqImg,2)]);  % 各切分子帧数
correctFramesSubIdx = [correctFrames(1,1),correctFrames(1,2:end) - frameCutIdx(1,:)];
    % 切分后小关键帧的内部索引

for frameCutNum = 1:length(correctFrames)
    CFSubIdx = correctFramesSubIdx(1,frameCutNum);   % 该切分中关键帧的索引
    subFrameCutIdxFront = frameCutIdxFront(1,frameCutNum);  % 切分段第一帧对应总帧索引
    subFrameLen = frameCutLen(1,frameCutNum);   % 切分段的子帧数量
    subFrameCut = timeFreqImg(:,(subFrameCutIdxFront:subFrameCutIdxFront + subFrameLen - 1)); % 提取该切分段对应所有帧
    subFrameCutDbscan = TFI_dbscan(:,(subFrameCutIdxFront:subFrameCutIdxFront + subFrameLen - 1));
        % 提取该切分段对应所有帧的聚类结果
    
    subTFILineIdx = zeros([TFILineNum,subFrameLen]);
        % 切分段中的曲线提取结果
    
    % 提取关键帧中TFILineNum条曲线的最大频率值对应索引
    for TFILineSubIdx = 1:TFILineNum
        subFrameCutDbscanFront...
            = find(subFrameCutDbscan(:,CFSubIdx)==TFILineSubIdx,1,'first');
            % 关键帧对应第几类的第一帧
        [subFrameCutDbscanMaxValue,subFrameCutDbscanMaxIdx]...
            = max(...
                subFrameCut(subFrameCutDbscan(:,CFSubIdx)==TFILineSubIdx...
                ,CFSubIdx)...
            );   % 关键帧对应第几类中的最大值
        subTFILineIdx(TFILineSubIdx,CFSubIdx)...
            = subFrameCutDbscanFront + subFrameCutDbscanMaxIdx - 1;
    end
    
    for subFrameCutIdx = (CFSubIdx-1):-1:1  % 关键帧左侧提取
        for TFILineSubIdx = 1:TFILineNum
            freqSearchCenter = subTFILineIdx(TFILineSubIdx,subFrameCutIdx + 1);
                % 以上一帧搜索到的频率为中心，两边freqSearchBiasBins为范围取max
            freqLeftIdx = freqSearchCenter - freqSearchBiasBins;
            freqRightIdx = freqSearchCenter + freqSearchBiasBins;
            freqLeftIdx(freqLeftIdx < 1) = 1;
            freqRightIdx(freqRightIdx > size(subFrameCut,1)) = size(subFrameCut,1);
            [subFrameCutDbscanMaxValue,subFrameCutDbscanMaxIdx]...
                = max(...
                    subFrameCut(freqLeftIdx:freqRightIdx,subFrameCutIdx)...
                );
            subTFILineIdx(TFILineSubIdx,subFrameCutIdx)...
                = freqLeftIdx + subFrameCutDbscanMaxIdx - 1;
        end
    end
    
    for subFrameCutIdx = (CFSubIdx+1):subFrameLen  % 关键帧右侧提取
        for TFILineSubIdx = 1:TFILineNum
            freqSearchCenter = subTFILineIdx(TFILineSubIdx,subFrameCutIdx - 1);
                % 以上一帧搜索到的频率为中心，两边freqSearchBiasBins为范围取max
            freqLeftIdx = freqSearchCenter - freqSearchBiasBins;
            freqRightIdx = freqSearchCenter + freqSearchBiasBins;
            freqLeftIdx(freqLeftIdx < 1) = 1;
            freqRightIdx(freqRightIdx > size(subFrameCut,1)) = size(subFrameCut,1);
            [subFrameCutDbscanMaxValue,subFrameCutDbscanMaxIdx]...
                = max(...
                    subFrameCut(freqLeftIdx:freqRightIdx,subFrameCutIdx)...
                );
            subTFILineIdx(TFILineSubIdx,subFrameCutIdx)...
                = freqLeftIdx + subFrameCutDbscanMaxIdx - 1;
        end
    end
    
    TFILineIdx(:,...
        subFrameCutIdxFront:subFrameCutIdxFront + subFrameLen - 1)...
        = int32(subTFILineIdx);   % 将切分段中提取到的曲线索引存入总曲线缓存中
end
%% plot
if drawFlag
    f1 = figure;
    f1.Position(1,3:4) = [850,300];

    f1_2 = subplot(2,2,2,'Parent',f1);
    imagesc(f1_2,TFI_cfar);
    xticklabels(f1_2,timeFreq_labelT(f1_2.XTick,1));
    yticklabels(f1_2,round(timeFreq_labelF(f1_2.YTick,1)));
    xlabel(f1_2,'Time (s)');
    ylabel(f1_2,'Frequency (Hz)');
    title(f1_2,'(b)');

    f1_3 = subplot(2,2,3,'Parent',f1);
    imagesc(f1_3,TFI_dbscan);
    xticklabels(f1_3,timeFreq_labelT(f1_3.XTick,1));
    yticklabels(f1_3,round(timeFreq_labelF(f1_3.YTick,1)));
    xlabel(f1_3,'Time (s)');
    ylabel(f1_3,'Frequency (Hz)');
    title(f1_3,'(c)');
    
    f1_1 = subplot(2,2,1,'Parent',f1);
    imagesc(f1_1,timeFreqImg);
    xticklabels(f1_1,timeFreq_labelT(f1_1.XTick,1));
    yticklabels(f1_1,round(timeFreq_labelF(f1_1.YTick,1)));
    xlabel(f1_1,'Time (s)');
    ylabel(f1_1,'Frequency (Hz)');
    title(f1_1,'(a)');
    
    f1_4 = subplot(2,2,4,'Parent',f1);
    imagesc(f1_4,timeFreqImg);
    xticklabels(f1_4,timeFreq_labelT(f1_4.XTick,1));
    yticklabels(f1_4,round(timeFreq_labelF(f1_4.YTick,1)));
    xlabel(f1_4,'Time (s)');
    ylabel(f1_4,'Frequency (Hz)');
    title(f1_4,'(d)');

    hold(f1_4,'on');
    for TFILineSubIdx = 1:TFILineNum
        plot(f1_4,TFILineIdx(TFILineSubIdx,:),'LineWidth',1);
    end
end
%% 找交叉点
% 找交叉点需要至少有两条交叉曲线
% freqCross = 0.04;   % 以该比例乘以总频率单元格数作为判定交叉点的距离
freqCrossBins = floor(freqCross * size(timeFreqImg,1)); % 交叉点单元格距离

crossPtsFlag = false;   % 是否有交叉点

if (TFILineNum >= 2)
    TFILineDiff = TFILineIdx(2:end,:) - TFILineIdx(1:(end-1),:);
else
    TFILineDiff = (freqCrossBins + 1) * ones([1,size(timeFreqImg,2)]);
end

TFILineCross = TFILineDiff <= freqCrossBins;  % 距离小，判断为交叉点
if any(TFILineCross,'all')
    crossPtsFlag = true;
else
    crossPtsFlag = false;
end
%% 新版交叉点合并
if crossPtsFlag
    % 直接从TFILineCross生成合并后交叉点TFILineCrossNumMerge，采用聚类方式
    % mergeFreq = 0.08;   % 需要合并交叉点的平均频率单元格间距的比例
    mergeFreqBins = floor(mergeFreq * size(timeFreqImg,1)); % 上述单元格间距
    % mergeTime = 1;   % 需要合并交叉点的帧数单元格间距，这里必须大于等于1，否则两个交叉点连着会出bug
    
    crossPtsIdx = int32(zeros(nnz(TFILineCross),2));   % 所有交叉点的涉及点的真实索引，即其在TF图中的位置
    % 第一列表示TF图第一维，第二列表示第二维，第一维为频率维，第二维为时间维
    TFILineCrossNumMerge = int32(zeros(size(TFILineCross)));  % 合并编号后的交叉点，这里可以选择更优的数据存储方式，但是为了使用原有代码，故没有修改
    
    [crossPtsIdxTmp_1,crossPtsIdxTmp_2] = find(TFILineCross);   % 找到其中所有涉及点的曲线上索引
    crossPtsIdxTmp_1 = reshape(crossPtsIdxTmp_1,[],1);
    crossPtsIdxTmp_2 = reshape(crossPtsIdxTmp_2,[],1);
    crossPtsIdx(:,2) = crossPtsIdxTmp_2;   % 第二维应对应时间索引
    
    for crossPtIdx = 1:nnz(TFILineCross)
        linePtIdxTmp_1 = crossPtsIdxTmp_1(crossPtIdx,1);   % 曲线号索引
        linePtIdxTmp_2 = crossPtsIdxTmp_2(crossPtIdx,1);   % 曲线时间索引
    
        crossPtsIdx(crossPtIdx,1) = round((TFILineIdx(linePtIdxTmp_1,linePtIdxTmp_2) + TFILineIdx(linePtIdxTmp_1 + 1,linePtIdxTmp_2)) / 2);
        % 取两条曲线频率索引平均值的四舍五入为交叉点真实频率索引
    end
    
    crossPtsClass = slowdbscan_dist(crossPtsIdx,mergePts,[mergeFreqBins;mergeTime]); % 使用聚类将各交叉点的涉及点区分开来
    
    for crossPtIdx = 1:nnz(TFILineCross)
        TFILineCrossNumMerge(crossPtsIdxTmp_1(crossPtIdx,1),crossPtsIdxTmp_2(crossPtIdx,1)) = int32(crossPtsClass(crossPtIdx,1));
        % 将各交叉点聚类索引号填入
    end
end

%% 准备交叉点绘图位置
if crossPtsFlag
    TFILineCrossNumCount = max(crossPtsClass,[],'all');  % 交叉点聚类类别数，即交叉点数
    
    drawPtsIdx = zeros(TFILineCrossNumCount,2);  % 所有画出交叉点
    
    for clsIdx = 1:TFILineCrossNumCount
        clsCrossPt = (crossPtsClass == clsIdx);  % 该交叉点的所有涉及点逻辑索引
        clsCrossPts = crossPtsIdx(clsCrossPt,:);   % 提取出所有涉及点
        if any(clsCrossPt)   % 该交叉点有涉及点
            TFICrossPtMaxValue = 0;
            TFICrossPtMaxIdx = 0;
            for clsCrossPtIdx = 1:nnz(clsCrossPt)  % 找出所有涉及点中在TF图中值最大的
                tmpValue = abs(timeFreqImg(clsCrossPts(clsCrossPtIdx,1),clsCrossPts(clsCrossPtIdx,2)));
                if tmpValue >= TFICrossPtMaxValue
                    TFICrossPtMaxValue = tmpValue;
                    TFICrossPtMaxIdx = clsCrossPtIdx;
                end
            end
            
            drawPtsIdx(clsIdx,:) = clsCrossPts(TFICrossPtMaxIdx,:);
        end
    end
end

%% 编号交叉点
% TFILineCrossNum = -1 * TFILineCross;  % 交叉点编号缓冲，-1表示待编号，0表示不是交叉点
% TFILineCrossNumCount = 0;    % 当前编号，也即总共交叉点计数
% % 从上到下（行），从左到右（列）将所有连续的交叉点编号
% for TFILineCrossIdx_2 = 1:size(TFILineCross,2)
%     for TFILineCrossIdx_1 = 1:size(TFILineCross,1)
%         if TFILineCrossNum(TFILineCrossIdx_1,TFILineCrossIdx_2) >= 0
%             continue;
%         else
%             TFILineCrossNumCount = TFILineCrossNumCount + 1;  % 交叉点计数
%             TFILineCrossIdx_2_Tmp = TFILineCrossIdx_2;  % 为方便从该出开始连续编号
%             while (TFILineCrossIdx_2_Tmp <= size(TFILineCross,2)) % 连续编号
%                 if TFILineCrossNum(TFILineCrossIdx_1,TFILineCrossIdx_2_Tmp) == -1
%                     TFILineCrossNum(TFILineCrossIdx_1,TFILineCrossIdx_2_Tmp) = TFILineCrossNumCount;
%                 else
%                     break;
%                 end
%                 TFILineCrossIdx_2_Tmp = TFILineCrossIdx_2_Tmp + 1;
%             end
%         end
%     end
% end
%% 合并交叉点
% % mergeFreq = 0.08;   % 需要合并交叉点的平均频率单元格间距的比例
% mergeFreqBins = floor(mergeFreq * size(timeFreqImg,1)); % 上述单元格间距
% % mergeTime = 1;   % 需要合并交叉点的帧数单元格间距，这里必须大于等于1，否则两个交叉点连着会出bug
% 
% crossPoints = zeros([TFILineCrossNumCount,4]);
%     % 所有交叉点，第一列为交叉点行数，第二列为起始列数，
%     % 第三列为持续帧长度，第四列表示该交叉点是否已被合并（1表示已被合并，跳过）
% TFILineCrossNumMerge = TFILineCrossNum;   % 合并完后的交叉点
% 
% TFILineCrossNumTrans = TFILineCrossNum.';   % 转制后方便find计算行数
% for TFILineCrossNumIdx = 1:TFILineCrossNumCount
%     crossPointsIdx = find(TFILineCrossNumTrans == TFILineCrossNumIdx);  % 转制后find
%     crossPointsLineIdx = idivide(int32(crossPointsIdx(1,1)),int32(size(TFILineCrossNum,2)),'ceil');  % 行数
%     crossPointsColIdx = crossPointsIdx(1,1) - (crossPointsLineIdx - 1) * size(TFILineCrossNum,2);  % 列数起始
%     crossPointsLineLen = length(crossPointsIdx);   % 长度
%     crossPoints(TFILineCrossNumIdx,1) = crossPointsLineIdx;
%     crossPoints(TFILineCrossNumIdx,2) = crossPointsColIdx;
%     crossPoints(TFILineCrossNumIdx,3) = crossPointsLineLen;
%     % disp(strcat(num2str(crossPointsLineIdx),',',num2str(crossPointsLColIdx),',',num2str(crossPointsLineLen)));
% end
% 
% for TFILineCrossNumIdx = 1:TFILineCrossNumCount
%     if crossPoints(TFILineCrossNumIdx,4)
%         continue;
%     else
%         TFILineCrossNumIdxStart = (crossPoints(TFILineCrossNumIdx,2) - mergeTime);
%         TFILineCrossNumIdxEnd = (crossPoints(TFILineCrossNumIdx,2) + crossPoints(TFILineCrossNumIdx,3) + mergeTime - 1);
%         if TFILineCrossNumIdxStart < 1
%             TFILineCrossNumIdxStart = 1;
%         end
%         if TFILineCrossNumIdxEnd > size(TFILineCrossNum,2)
%             TFILineCrossNumIdxEnd = size(TFILineCrossNum,2);
%         end
%         subLineCross = TFILineCrossNum(:,...
%             TFILineCrossNumIdxStart...
%             :TFILineCrossNumIdxEnd);
%             % 切分出该区域内所有交叉点，一个一个处理
%         lineCrossFreqMean = sum(...
%             TFILineIdx(crossPoints(TFILineCrossNumIdx,1)...
%                 ,(crossPoints(TFILineCrossNumIdx,2))...
%                     :(crossPoints(TFILineCrossNumIdx,2) + crossPoints(TFILineCrossNumIdx,3) - 1))...
%             + TFILineIdx(crossPoints(TFILineCrossNumIdx,1) + 1 ...
%                 ,(crossPoints(TFILineCrossNumIdx,2))...
%                     :(crossPoints(TFILineCrossNumIdx,2) + crossPoints(TFILineCrossNumIdx,3) - 1))...
%             ,'all') / (crossPoints(TFILineCrossNumIdx,3) * 2);
%         
%         toMergeCrossIdx = (subLineCross > 0) & (subLineCross ~= TFILineCrossNumIdx);  % 待检验交叉点的逻辑索引矩阵
%         toMergeCross = reshape(unique(subLineCross(toMergeCrossIdx)),[],1);  % 待检验交叉点编号
%         
%         for toMergeCrossTmp = toMergeCross  % 一个一个检验交叉点平均频率是否相近
%             toMergeCrossFreqMean = sum(...
%                 TFILineIdx(crossPoints(toMergeCrossTmp,1)...
%                     ,(crossPoints(toMergeCrossTmp,2))...
%                         :(crossPoints(toMergeCrossTmp,2) + crossPoints(toMergeCrossTmp,3) - 1))...
%                 + TFILineIdx(crossPoints(toMergeCrossTmp,1) + 1 ...
%                     ,(crossPoints(toMergeCrossTmp,2))...
%                         :(crossPoints(toMergeCrossTmp,2) + crossPoints(toMergeCrossTmp,3) - 1))...
%                 ,'all') / (crossPoints(toMergeCrossTmp,3) * 2);
%             
%             if abs(lineCrossFreqMean - toMergeCrossFreqMean) <= mergeFreqBins
%                 % 平均频率小于间距，两交叉点合并
%                 TFILineCrossNumMerge(TFILineCrossNum == toMergeCrossTmp) = TFILineCrossNumIdx;
%                     % 直接修改
%                 crossPoints(toMergeCrossTmp,4) = 1;    % 标记该交叉点已使用
%                 % disp(strcat('第',num2str(toMergeCrossTmp),'交叉点合并到第',...
%                 %     num2str(TFILineCrossNumIdx),'交叉点'));
%             end
%         end
%     end
% end
%% 整理出各曲线对应交叉点
if crossPtsFlag
    TFILineCrossNumEach = int32(zeros(size(TFILineIdx)));  % 各曲线中包含的交叉点索引，每行交叉点处索引可能一致
    
    TFILineCrossNumPad = zeros([size(TFILineCrossNumMerge,1) + 2,size(TFILineCrossNumMerge,2)]);
        % TFILineCrossNum上面下面各补一行0
    TFILineCrossNumPad(2:(end - 1),:) = TFILineCrossNumMerge;
    
    for TFILineSubIdx = 1:TFILineNum
        TFILineCrossNumPadSub = TFILineCrossNumPad(TFILineSubIdx:(TFILineSubIdx + 1),:);
        for timeIdx = 1:size(TFILineCrossNumEach,2)
            TFILineCrossNumEach(TFILineSubIdx,timeIdx) = int32(max(TFILineCrossNumPadSub(:,timeIdx),[],'all'));
                % 将最大的交叉点索引存入
        end
    end
end
%% plot未合并的交叉点
% hold(f1_4,'on');
% for TFILineCrossNumIdx = 1:TFILineCrossNumCount
%     crossPointsLineIdx = crossPoints(TFILineCrossNumIdx,1);
%     crossPointsColIdx = crossPoints(TFILineCrossNumIdx,2);
%     lineCrossFreqMean = sum(...
%         TFILineIdx(crossPoints(TFILineCrossNumIdx,1)...
%             ,(crossPoints(TFILineCrossNumIdx,2))...
%                 :(crossPoints(TFILineCrossNumIdx,2) + crossPoints(TFILineCrossNumIdx,3) - 1))...
%         + TFILineIdx(crossPoints(TFILineCrossNumIdx,1) + 1 ...
%             ,(crossPoints(TFILineCrossNumIdx,2))...
%                 :(crossPoints(TFILineCrossNumIdx,2) + crossPoints(TFILineCrossNumIdx,3) - 1))...
%         ,'all') / (crossPoints(TFILineCrossNumIdx,3) * 2);
%     scatter(f1_4,crossPointsColIdx,lineCrossFreqMean,'ro');
% end
%% 新版plot合并交叉点
if crossPtsFlag
    if drawFlag
        hold(f1_4,'on');
        scatter(f1_4,drawPtsIdx(:,2),drawPtsIdx(:,1),280,'r.');
    end
end
%% 计算各交叉点前后配对曲线索引
% 重组曲线需要至少有两条交叉曲线
if crossPtsFlag
    % diffMeanBins = 3;    % 计算左右侧斜率使用单元格（帧）数量
    diffFreqBiasBins = floor(diffFreqBias * size(timeFreqImg,1));    % 交叉点左右斜率偏差小于等于该值都认为斜率一致
    
    TFILineNumVec = reshape(1:TFILineNum,1,[]);  % TFILineNum数量的顺序索引矩阵
    rebuildCross = repmat(TFILineNumVec,TFILineCrossNumCount,1);  % 行索引表示第几个交叉点，各列表示该列重组接到交叉点后的第几条曲线
        % 如[2,1,3,4]表示1、2条曲线换位，但3、4不变。
        % 先将rebuildCross每列均赋1:TFILineNum，表示先全部接之前的曲线索引
    for TFILineCrossNumIdx = 1:TFILineCrossNumCount
        crossMergeMask = (TFILineCrossNumMerge == TFILineCrossNumIdx);
        if any(crossMergeMask,'all')  % 有该序号表示该交叉点未被合并
            mergedCrossPointsLine = reshape(find(sum(crossMergeMask,2)),[],1);  % 提取出交叉的两条曲线索引
            toMergeLineAll = zeros([length(mergedCrossPointsLine),2]);  % 曲线索引缓冲器
            toMergeLineAll(:,1) = mergedCrossPointsLine;
            toMergeLineAll(:,2) = mergedCrossPointsLine + 1;
            % 提取出有交叉点的曲线索引，即将两条对应一个索引转换为一一对应
            
            toMergeLine = reshape(unique(toMergeLineAll),1,[]);
            
            crossMergeNow = find(crossMergeMask);  % 对应到该合并交叉点的具体线性索引
            leftColIdx = idivide(int32(min(crossMergeNow,[],'all')),int32(size(TFILineCrossNumMerge,1)),'ceil');
                % 该合并交叉点的左侧所在列
            rightColIdx = idivide(int32(max(crossMergeNow,[],'all')),int32(size(TFILineCrossNumMerge,1)),'ceil');
                % 该合并交叉点的右侧所在列
            centerColIdx = int32(round((leftColIdx + rightColIdx) / 2));  % 该交叉点的中心列
        else
            toMergeLine = zeros([1,0]);
            leftColIdx = 0;
            rightColIdx = 0;
            centerColIdx = 0;
        end
        
        diffLeftStartIdx = centerColIdx - diffMeanBins;
            % 这里可将centerColIdx换为leftColIdx以从交叉点的最左侧开始计算，否则从中心开始计算
        diffLeftEndIdx = centerColIdx - 1;
            % 这里可将centerColIdx换为leftColIdx以从交叉点的最左侧开始计算，否则从中心开始计算
        diffRightStartIdx = centerColIdx + 1;
            % 这里可将centerColIdx换为rightColIdx以从交叉点的最右侧开始计算，否则从中心开始计算
        diffRightEndIdx = centerColIdx + diffMeanBins;
            % 这里可将centerColIdx换为rightColIdx以从交叉点的最右侧开始计算，否则从中心开始计算
        if diffLeftStartIdx < 1
            diffLeftStartIdx = 1;
        end
        if diffLeftEndIdx < 1
            diffLeftEndIdx = 1;
        end
        if abs(diffLeftEndIdx - diffLeftStartIdx) < 2  % 没有足够计算斜率单元格
            continue;
        end
        if diffRightStartIdx > size(TFILineIdx,2)
            diffRightStartIdx = size(TFILineIdx,2);
        end
        if diffRightEndIdx > size(TFILineIdx,2)
            diffRightEndIdx = size(TFILineIdx,2);
        end
        if abs(diffRightEndIdx - diffRightStartIdx) < 2  % 没有足够计算斜率单元格
            continue;
        end
        
        mergeLineDiffComp = zeros([TFILineNum,2]);  % 第一列表示该交叉点的交换行的交叉点前平均斜率，第二列表示交叉点后平均斜率
            % 该交叉点没有经过的曲线就不计算，将斜率保持为0
        
        % 交叉，计算前后斜率
        mergeLineDiffComp(toMergeLine,1)...
            = mean(diff(TFILineIdx(toMergeLine,diffLeftStartIdx:diffLeftEndIdx),1,2),2);  % 左侧斜率
        mergeLineDiffComp(toMergeLine,2)...
            = mean(diff(TFILineIdx(toMergeLine,diffRightStartIdx:diffRightEndIdx),1,2),2);  % 右侧斜率
        
        lineExchangeUsageMask = false([1,TFILineNum]);   % 交换右侧曲线使用情况，使用标0，未使用为1
        lineExchangeUsageMask(1,toMergeLine) = true;   % 将需要交换的标1，不需要直接标0
        
        mergeLineDiffLeftMesh = repmat(reshape(mergeLineDiffComp(toMergeLine,1),[],1),1,length(toMergeLine));
        mergeLineDiffRightMesh = repmat(reshape(mergeLineDiffComp(toMergeLine,2),1,[]),length(toMergeLine),1);
        
        mergeLineDiffMesh = abs(mergeLineDiffLeftMesh - mergeLineDiffRightMesh);   % 各曲线索引斜率相减
        
        % 下面执行曲线配对策略，优化目标为总斜率差最小
        % 先搜索对角线上小于等于diffFreqBias的，认为其为左右曲线一致，直接采用
        % 然后每行搜索一个最小值
        
        % 再找对角线上小于diffFreqBias的单元格的列，该列索引即为接第几条曲线
        eqDiffLine = reshape((mergeLineDiffMesh(logical(eye(length(toMergeLine)))) <= diffFreqBiasBins),1,[]);
            % 提取出左右相同斜率的同一根曲线对应第几根
        mergeLineDiffMeshTmp = mergeLineDiffMesh;   % 暂存
        mergeLineDiffMeshTmp(:,eqDiffLine) = inf;   % 将选用列直接赋值为inf，取最小值不取
            % 找各列最小值，即为第（行索引）曲线接第（列索引）曲线
        [lineMaxValue,lineMaxIdx] = min(mergeLineDiffMeshTmp,[],2);   % 找各行最小值和最小值对应列索引
        toUseLine = reshape(lineMaxIdx,1,[]);    % 暂存，toUseLine作为第（索引）曲线右接第（值）曲线
        toUseLine(eqDiffLine) = reshape(find(eqDiffLine),1,[]);   % 将上述对角线上相等的索引存入
        
        rebuildCross(TFILineCrossNumIdx,toMergeLine) = toMergeLine(1,toUseLine);   % 存入右曲线使用索引
    end
end
%% 重组曲线，拼出曲线频率索引值
% 一根一根一帧一帧拼
if crossPtsFlag
    TFILineIdxRebuild = int32(zeros(size(TFILineIdx)));   % 重建后的曲线频率索引
    
    TFILineCrossNumEachTrans = TFILineCrossNumEach.';   % 为方便计算行索引，转置之
    
    for TFILineSubIdx = 1:TFILineNum
        nextLineNum = TFILineSubIdx;   % 下一帧应取的曲线索引
        timeIdx = int32(1);
        while timeIdx <= size(TFILineIdx,2)
            crossPointIdx = TFILineCrossNumEach(nextLineNum,timeIdx);  % 交叉点索引
            if crossPointIdx <= 0
                % 小于等于0表示没有交叉点，直接赋值
                TFILineIdxRebuild(TFILineSubIdx,timeIdx) = TFILineIdx(nextLineNum,timeIdx);
                    % 注意，前面是重建曲线，曲线索引每次应为一致，后面为原始曲线，索引应不一致
                timeIdx = timeIdx + 1;
            else
                % 这里一次性线性插值完成，直接赋值后时间索引向后推
                startLineNum = nextLineNum;   % 上一帧曲线索引
                nextLineNum = rebuildCross(crossPointIdx,startLineNum);  % 下一帧曲线索引
                
                crossPointMask = (TFILineCrossNumEach == crossPointIdx);
                crossPointNow = find(crossPointMask);  % 对应到该合并交叉点的具体线性索引
                leftColIdx = idivide(int32(min(crossPointNow,[],'all')),int32(size(TFILineCrossNumEach,1)),'ceil');
                    % 该合并交叉点的左侧所在列
                rightColIdx = idivide(int32(max(crossPointNow,[],'all')),int32(size(TFILineCrossNumEach,1)),'ceil');
                    % 该合并交叉点的右侧所在列
                crossPointLen = abs(rightColIdx - leftColIdx + 1);
                
                startLineFreqBinIdx = TFILineIdx(startLineNum,timeIdx);
                endLineFreqBinIdx = TFILineIdx(nextLineNum,timeIdx + crossPointLen - 1);
    
                if crossPointLen <= 1
                    freqBinValueTmp = startLineFreqBinIdx;
                else
                    freqBinValueTmp...
                        = interp1(double([timeIdx,timeIdx + crossPointLen - 1]) ...
                            ,double([startLineFreqBinIdx,endLineFreqBinIdx]) ...
                            ,double((timeIdx:(timeIdx + crossPointLen - 1))) ...
                        );   % 插值后值
                end
    
                TFILineIdxRebuild(TFILineSubIdx,timeIdx:timeIdx + crossPointLen - 1) = int32(round(freqBinValueTmp));  % 线性插值
    
                timeIdx = timeIdx + crossPointLen;
            end
            
        end
    end
end

if not(crossPtsFlag)
    TFILineIdxRebuild = TFILineIdx;
end
%% plot最终结果
if drawFlag
    f2 = figure;
    f2.Position(1,3:4) = [400,300];
    f2_1 = axes(f2);
    hold(f2_1,'on');
    for TFILineSubIdx = 1:TFILineNum
        plot(f2_1,TFILineIdxRebuild(TFILineSubIdx,:),'LineWidth',1);
    end
    xlim(f2_1,[1,size(timeFreq_labelT,1)]);
    ylim(f2_1,[1,size(timeFreq_labelF,1)]);
    xticklabels(f2_1,timeFreq_labelT(f2_1.XTick,1));
    yticklabels(f2_1,round(timeFreq_labelF(f2_1.YTick,1)));
    xlabel(f2_1,'Time (s)');
    ylabel(f2_1,'Frequency (Hz)');
end
%% plot旋转角速度分析结果
disp(strcat('螺旋桨旋转频率=',num2str(bladeRate/2/pi),'圈/秒'));

minPossibleRotorFreq = 1;   % 旋转频率最小可能值，从该值以上寻找最大值确认为旋转频率

fftPoints = 2048;
TFISampleFreq = 1 / mean(diff(timeFreq_labelT),'all');
freqAxis = linspace((-1 * TFISampleFreq / 2),(TFISampleFreq / 2),fftPoints);
possibleRotorFreqMask = (freqAxis >= minPossibleRotorFreq);
TFILineFFT = complex(zeros([TFILineNum,fftPoints]));
rotorFreq = zeros([1,TFILineNum]);
for TFILineSubIdx = 1:TFILineNum
    TFILineFFT(TFILineSubIdx,:) = fftshift(fft(TFILineIdxRebuild(TFILineSubIdx,:),fftPoints));
    
    [maxValue,maxIdx] = max(abs(TFILineFFT(TFILineSubIdx,possibleRotorFreqMask)));
    freqAxisTmp = freqAxis(1,possibleRotorFreqMask);
    rotorFreq(1,TFILineSubIdx) = freqAxisTmp(1,maxIdx);
end

rotorFreqMean = mean(rotorFreq,'all');
disp(strcat('螺旋桨旋转频率估计=',num2str(rotorFreqMean),'圈/秒'));

if drawFlag
    f3 = figure;
    f3.Position(1,3:4) = [400,300];
    f3_1 = axes(f3);
    hold(f3_1,'on');
    for TFILineSubIdx = 1:TFILineNum
        plot(f3_1,freqAxis,abs(TFILineFFT(TFILineSubIdx,:)),'LineWidth',1);
    end
    xlim(f3_1,[-10,10]);
    xlabel(f3_1,'Frequency (Hz)');
    ylabel(f3_1,'Amplitude');
end
%% disp最大频移
disp(strcat('螺旋桨旋转最大频移=2*fc*v/c=',num2str(2*carrFreq*(bladeLen*bladeRate)/lightSpeed)));

maxFreq = timeFreq_labelF(max(abs(TFILineIdxRebuild),[],'all'),1);
disp(strcat('最大频移为',num2str(maxFreq)));
%% 计算多普勒频率真值
fftPts = length(timeFreq_labelF);   % stft的fft点数
mdTruth = (mod((doppler + (frameFreq / 2)),frameFreq) / frameFreq) * fftPts;  % 微动频率真值，这里对应为频率单元格索引，但有小数
%% 真值绘制
if drawFlag
    f4 = figure;
    f4.Position(1,3:4) = [400,300];
    f4_1 = axes(f4);
    hold(f4_1,'on');
    for mdTruthIdx = 1:size(mdTruth,1)
        plot(f4_1,mdTruth(mdTruthIdx,:),'LineWidth',1);
    end
    xlim(f4_1,[1,frameNums]);
    ylim(f4_1,[1,size(timeFreq_labelF,1)]);
    yticklabels(f4_1,round(timeFreq_labelF(f4_1.YTick,1)));
    xlabel(f4_1,'Frame Index');
    ylabel(f4_1,'Frequency (Hz)');
end
%% 真值模糊处理，将其与stft帧一一对应
shortPts = round((timeLen - timeFreq_labelT(end,1)) * frameFreq) * 2;   % stft每一帧的实际使用采样点数
shortInterPts = round((timeFreq_labelT(end,1) - timeFreq_labelT(end-1,1)) * frameFreq);   % 每一帧之间的间隔
shortFrameNums = length(timeFreq_labelT);   % stft的总帧数

shortFrameIdx = zeros(shortFrameNums,shortPts);
shortFrameIdx(:,:) = bsxfun(@plus,reshape(flip(frameNums - shortPts + 1:-1*shortInterPts:1,2),[],1),shortFrameIdx);  % 各stft帧的起始采样点索引

frameIdxTmp = (0:shortPts-1);
shortFrameIdx = bsxfun(@plus,shortFrameIdx,frameIdxTmp);

mdTruthShort = zeros(size(mdTruth,1),shortFrameNums);
for mdTruthIdx = 1:size(mdTruth,1)
    mdTruthShortTmp = reshape(mdTruth(mdTruthIdx,shortFrameIdx),size(shortFrameIdx));
    mdTruthShort(mdTruthIdx,:) = reshape(round(mean(mdTruthShortTmp,2)),1,[]);
end
%% 模糊后真值绘制
if drawFlag
    f5 = figure;
    f5.Position(1,3:4) = [400,300];
    f5_1 = axes(f5);
    hold(f5_1,'on');
    for mdTruthIdx = 1:size(mdTruth,1)
        plot(f5_1,mdTruthShort(mdTruthIdx,:),'LineWidth',1);
    end
    xlim(f5_1,[1,size(timeFreq_labelT,1)]);
    ylim(f5_1,[1,size(timeFreq_labelF,1)]);
    xticklabels(f5_1,timeFreq_labelT(f5_1.XTick,1));
    yticklabels(f5_1,round(timeFreq_labelF(f5_1.YTick,1)));
    xlabel(f5_1,'Time (s)');
    ylabel(f5_1,'Frequency (Hz)');
end
%% 计算统计误差
% 这里采用ospa距离来统计误差，使用平均ospa距离来衡量某个信噪比下提取曲线与真实微多普勒频移之间的差
ospa_vals = zeros(shortFrameNums,3);
% 第一列为OSPA Dist，第二列为OSPA Loc，第三列为OSPA Card
ospa_c = 1;   % 关联敏感性参数
ospa_p = 1;   % 距离敏感性参数
for shortFrameIdx = 1:shortFrameNums
    [ospa_vals(shortFrameIdx,1),...
    ospa_vals(shortFrameIdx,2),...
    ospa_vals(shortFrameIdx,3)] = ...
        ospa_dist(mdTruthShort(:,shortFrameIdx).', ...
        double(TFILineIdxRebuild(:,shortFrameIdx)).', ...
        ospa_c,ospa_p);
    % 注意：ospa_dist的前两个变量接收参数为一矩阵，其中每列为一个元素，矩阵对应的势为列的数量
end
%% 平均误差
ospaAvg = mean(ospa_vals,1);
disp(strcat('信噪比为',num2str(SNRdb),'dB时，', ...
    '平均OSPA距离为',num2str(ospaAvg(1,1)), ...
    '，平均数值误差为',num2str(ospaAvg(1,2)), ...
    '，平均势误差为',num2str(ospaAvg(1,3)),'。'));
%% 辅助图片绘制
if drawFlag
    f6 = figure;
    f6.Position(1,3:4) = [400,300];
    f6_1 = axes(f6);
    imagesc(f6_1,timeFreqImg);
    xticklabels(f6_1,timeFreq_labelT(f6_1.XTick,1));
    yticklabels(f6_1,round(timeFreq_labelF(f6_1.YTick,1)));
    xlabel(f6_1,'Time (s)');
    ylabel(f6_1,'Frequency (Hz)');

    f7TimeIdx = 1;
    f7Data = timeFreqImg(:,f7TimeIdx);
    f7DataTh = zeros(size(f7Data));
    [cfarDet,cfarTh] = cfar(f7Data,cfarCutIdx);  % 按照上面设置的参数重新计算一次
    f7Detection = zeros(nnz(cfarDet),2);
    f7DataTh(cfarCutIdx,1) = cfarTh;
    f7Detection(:,1) = cfarCutIdx(1,cfarDet);
    f7Detection(:,2) = f7Data(cfarCutIdx(1,cfarDet),1);
    f7Idx = TFI_dbscan(f7Detection(:,1),f7TimeIdx);
    f7IdxValid = f7Idx > 0;

    f7 = figure;
    f7.Position(1,3:4) = [400,300];
    f7_1 = axes(f7);
    plot(f7_1,f7Data,'DisplayName','Frame Data');
    hold(f7_1,'on');
    plot(f7_1,f7DataTh,'-.','DisplayName','Threshold');
    scatter(f7_1,f7Detection(:,1),f7Detection(:,2),40,'r.','DisplayName','Detect points');
    xlim(f7_1,[1,size(timeFreq_labelF,1)]);
    ylim(f7_1,[min(f7Data,[],'all'),max(f7Data,[],'all')*1.1]);
    xticklabels(f7_1,round(timeFreq_labelF(f7_1.XTick,1)));
    xlabel(f7_1,'Frequency (Hz)');
    ylabel(f7_1,'Amplitude');
    legend(f7_1);

    f8 = figure;
    f8.Position(1,3:4) = [400,300];
    f8_1 = axes(f8);
    gsh = gscatter(f8_1,f7Detection(f7IdxValid,1),f7Detection(f7IdxValid,2),f7Idx(f7IdxValid,1),[],'.',8,'off');
    for gshIdx = 1:size(gsh,1)
        gsh(gshIdx,1).DisplayName = strcat('Group',num2str(gshIdx));
    end
    hold(f8_1,'on');
    plot(f8_1,f7Data,'DisplayName','Frame Data');
    plot(f8_1,f7DataTh,'-.','DisplayName','Threshold');
    xlim(f8_1,[1,size(timeFreq_labelF,1)]);
    ylim(f8_1,[min(f7Data,[],'all'),max(f7Data,[],'all')*1.1]);
    xticklabels(f8_1,round(timeFreq_labelF(f8_1.XTick,1)));
    xlabel(f8_1,'Frequency (Hz)');
    ylabel(f8_1,'Amplitude');
    legend(f8_1);

    f9 = figure;
    f9.Position(1,3:4) = [400,300];
    f9_1 = axes(f9);
%     imagesc(f9_1,timeFreqImg);

    hold(f9_1,'on');
    for TFILineSubIdx = 1:TFILineNum
        plot(f9_1,TFILineIdx(TFILineSubIdx,:),'LineWidth',1);
    end
    xlabel(f9_1,'Time (s)');
    ylabel(f9_1,'Frequency (Hz)');
    xlim(f9_1,[1,size(timeFreq_labelT,1)]);
    ylim(f9_1,[1,size(timeFreq_labelF,1)]);
    xticklabels(f9_1,timeFreq_labelT(f9_1.XTick,1));
    yticklabels(f9_1,round(timeFreq_labelF(f9_1.YTick,1)));

    if crossPtsFlag
        scatter(f9_1,drawPtsIdx(:,2),drawPtsIdx(:,1),280,'r.');
    end

    f10 = figure;
    f10.Position(1,3:4) = [400,300];
    f10_1 = axes(f10);
    plot(f10_1,f7Data);
    xlim(f10_1,[1,size(timeFreq_labelF,1)]);
    ylim(f10_1,[min(f7Data,[],'all'),max(f7Data,[],'all')*1.1]);
    xticklabels(f10_1,round(timeFreq_labelF(f10_1.XTick,1)));
    xlabel(f10_1,'Frequency (Hz)');
    ylabel(f10_1,'Amplitude');

end

%% 清空outsideRun变量
clear('outsideRun');
%% 保存图
if and(drawFlag,ifSave)
    % 保存fig
    savefig(f1,fullfile(savePath,strcat(num2str(SNRdb),'db','_1')));
    savefig(f2,fullfile(savePath,strcat(num2str(SNRdb),'db','_2')));
    savefig(f3,fullfile(savePath,strcat(num2str(SNRdb),'db','_3')));
    savefig(f4,fullfile(savePath,strcat(num2str(SNRdb),'db','_4')));
    savefig(f5,fullfile(savePath,strcat(num2str(SNRdb),'db','_5')));
    savefig(f6,fullfile(savePath,strcat(num2str(SNRdb),'db','_6')));
    savefig(f7,fullfile(savePath,strcat(num2str(SNRdb),'db','_7')));
    savefig(f8,fullfile(savePath,strcat(num2str(SNRdb),'db','_8')));
    savefig(f9,fullfile(savePath,strcat(num2str(SNRdb),'db','_9')));
    savefig(f10,fullfile(savePath,strcat(num2str(SNRdb),'db','_10')));
    % 保存word可插入
    saveas(f1,fullfile(savePath,strcat(num2str(SNRdb),'db','_1')),'emf');
    saveas(f2,fullfile(savePath,strcat(num2str(SNRdb),'db','_2')),'emf');
    saveas(f3,fullfile(savePath,strcat(num2str(SNRdb),'db','_3')),'emf');
    saveas(f4,fullfile(savePath,strcat(num2str(SNRdb),'db','_4')),'emf');
    saveas(f5,fullfile(savePath,strcat(num2str(SNRdb),'db','_5')),'emf');
    saveas(f6,fullfile(savePath,strcat(num2str(SNRdb),'db','_6')),'emf');
    saveas(f7,fullfile(savePath,strcat(num2str(SNRdb),'db','_7')),'emf');
    saveas(f8,fullfile(savePath,strcat(num2str(SNRdb),'db','_8')),'emf');
    saveas(f9,fullfile(savePath,strcat(num2str(SNRdb),'db','_9')),'emf');
    saveas(f10,fullfile(savePath,strcat(num2str(SNRdb),'db','_10')),'emf');
end